%% SDP design for formation control gain matrix.
%
% Inputs:
%
%       - qsMat :  Desired formation coordinates.
%       - adj:  Graph adjacency matrix (n*n logical matrix)
%
% Outputs:
%
%       - Ar : Gain matrix
%
% (C) Tyler Summers and Kaveh Fathian, 2017-2018.
% Emails: tyler.summers@utdallas.edu, kavehfathian@gmail.com
%
%%
function [Ar] = FindGains_Ver1_4(qsMat, adj)

    % Add CVX to path
    addpath('CVX');
    cvx_setup

    % Transform matrix to vector
    qs = qsMat(:);

    % Number of agents
    n = size(qsMat, 2);

    % Complex representation of desired formation coordinates
    p = qs(1:2:end - 1);
    q = qs(2:2:end);
    z = p + 1i * q;

    % Get orthogonal complement of [z ones(n,1)]
    [U, ~, ~] = svd([z ones(n, 1)]);
    Q = U(:, 3:n);

    % Subspace constraint for the given graph
    S = not(adj);
    S = S - diag(diag(S));

    % Solve via CVX
    % NOTE: CVX must be downloaded and installed. See http://cvxr.com/cvx/
    cvx_begin sdp
    variable A(n, n) hermitian
    maximize(lambda_min(Q' * A * Q))
    subject to
    A * [z ones(n, 1)] == 0 + 1i * 0;
    norm(A) <= 10;
    A .* S == 0;
    cvx_end

    Ac = -full(A); % Complex gain matrix
    Ar = A_C2R(Ac); % Real represnetation of gain matrix

    % Transform a complex matrix to real representation
    function AnR = A_C2R(An)

        numA = size(An, 3);
        n = size(An, 1);

        AnR = zeros(2 * n, 2 * n, numA);

        for k = 1:numA

            for i = 1:n

                for j = 1:n

                    lij = An(i, j);
                    re = real(lij);
                    im = imag(lij);

                    AnR(2 * i - 1:2 * i, 2 * j - 1:2 * j, k) = [re, -im; im, re];

                end

            end

        end
